close all; clear; clc;

Alpha = [0, pi/2, 0, pi/2, -pi/2, pi/2];
A = [0, 0.18, 0.58, 0.16, 0, 0];
D = [0.43, 0, 0, 0.6395, 0, 0.115];
Theta = [0, pi/2, 0, 0, 0, 0];
Sigma = [1, 1, 1, 1, 1, 1];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;
DH_para.Sigma = Sigma;

for i = 1:6
    L(i) = Link('alpha',Alpha(i),'a', A(i),'d',D(i));
    L(i).offset = Theta(i); L(i).qlim=[-2 * pi, 2 * pi];L(i).mdh = 1;
end

robot = SerialLink(L, 'name', 'robot');

T0 = troty(pi/2) * trotz(-pi/3);
px = 0.934; pz = 1.17;
pys = -0.15; pyn = 0.15;
pzs = pz - 0.20; pzn = pz + 0.20;
Ts = T0 * transl(px, pys, pz);
Tn = T0 * transl(px, pyn, pz);

tn = 10;
dt = 0.1;
t = 0:dt:tn;

q0 = [0, 0, 0, 0, 0, 0];

num = length(t);

q1_1 = zeros(num, 1);
q2_1 = zeros(num, 1);
q3_1 = zeros(num, 1);
q4_1 = zeros(num, 1);
q5_1 = zeros(num, 1);
q6_1 = zeros(num, 1);

q1_2 = zeros(num, 1);
q2_2 = zeros(num, 1);
q3_2 = zeros(num, 1);
q4_2 = zeros(num, 1);
q5_2 = zeros(num, 1);
q6_2 = zeros(num, 1);

for i = 1:length(t)
    py = pys + (pyn - pys) * t(i) / tn;
    pz = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px, py, pz) * T0;
    qn = ikine(T, q0, DH_para, 0);
    
    q1_1(i) = qn(1);
    q2_1(i) = qn(2);
    q3_1(i) = qn(3);
    q4_1(i) = qn(4);
    q5_1(i) = qn(5);
    q6_1(i) = qn(6);
    
    qn = ikine(T, q0, DH_para, 1);
    
    q1_2(i) = qn(1);
    q2_2(i) = qn(2);
    q3_2(i) = qn(3);
    q4_2(i) = qn(4);
    q5_2(i) = qn(5);
    q6_2(i) = qn(6);
end

[q5_min, q5_min_index] = min(abs(q5_1));

q1 = q1_1; q1(q5_min_index:end) = q1_2(q5_min_index:end);
q2 = q2_1; q2(q5_min_index:end) = q2_2(q5_min_index:end);
q3 = q3_1; q3(q5_min_index:end) = q3_2(q5_min_index:end);
q4 = q4_1; q4(q5_min_index:end) = q4_2(q5_min_index:end);
q5 = q5_1; q5(q5_min_index:end) = q5_2(q5_min_index:end);
q6 = q6_1; q6(q5_min_index:end) = q6_2(q5_min_index:end);

interpo_num = 10;
q4_new = interpolate(q4, q5_min_index, interpo_num);

for i = 1:length(t)
    py = pys + (pyn - pys) * t(i) / tn;
    pz = pzs + (pzn - pzs) * t(i) / tn;
    T = transl(px, py, pz) * T0;
    if i < q5_min_index-interpo_num || i > q5_min_index+interpo_num
        qn = [q1(i), q2(i), q3(i), q4(i), q5(i), q6(i)];
    else 
        q0 = [q1(i), q2(i), q3(i), q4(i), q5(i), q0(6)];
        qn = ikine2(T, q0, DH_para, q4_new(i));
    end

    q0 = qn;
    q(i, :) = qn;
end

dq = diff(q) / dt;
ddq = diff(dq) / dt;

figure(1)
for i = 1:6
subplot(3, 2, i);
plot(t, q(:, i));
xlabel("t (s)");
ylabel("q" + i + " (rad)");
end

figure(2)
for i = 1:6
subplot(3, 2, i)
plot(t(1:end-1), dq(:, i));
xlabel("t (s)");
ylabel("dq" + i + " (rad/s)");
end

figure(3)
for i = 1:6
subplot(3, 2, i)
plot(t(1:end-2), ddq(:, i));
xlabel("t (s)");
ylabel("ddq" + i + " (rad/s^2)");
end

q0_timeseries = timeseries([q1_1, q2_1, q3_1, q4_1, q5_1, q6_1], t);
q1_timeseries = timeseries(q, t);
px_timeseries = timeseries(px, t);
py_timeseries = timeseries(py, t);
pz_timeseries = timeseries(pz, t);
